﻿// 平差函数模型的构建、法方程解算
// 包含方向、距离以及陀螺边函数模型

using System;
using System.Collections.Generic;
using System.Text;
using MathNet.Numerics.LinearAlgebra;
using static AdjPlane.Angle_t;
using static AdjPlane.AdjCmn;

namespace AdjPlane
{
    using mat = Matrix<double>;
    using obs_dic = Dictionary<string, double>;
    using poi_dic = Dictionary<string, Point2D>;

    //平差求解文件
    public partial class Plane
    {
        //1对应方向观测值，2对应距离观测值
        protected int ukp_num, p_num, nl = 0, ns = 0,   //未知点数，总点数,L，S观测值的个数；
        n, t, r;                                        //观测值数，参数个数；
        protected double alpha, msa, msb,               //方向精度，固定误差，比例误差
        qv, qvl, qvs,                                   //总体、方向、距离单位权方差
        sigL, sigS,                                     //L、S单位权中误差
        M0, sigma, fai,                                 //验前、验后单位权中误; fai根据是否COV，取M0或者sigma；
        PVV, PVV1, PVV2;
        protected mat P, P1, P2, Q1, Q2,                //观测值的权 观测值协方差
        A, A1, A2, L, L1, L2,                           //误差方程系数阵  常数项
        N, N1, N2, W, W1, W2,                           //法方程系数阵、常数项
        X, V, V1, V2,                                   //参数、改正数
        QX;                                             // 坐标协因数

        //陀螺边
        mat Vt, Nt, Wt, Pt, Qt;
        double PVVt;
        int nt;                                         //陀螺边个数
        double sigt;                                    //陀螺边的先验精度

        public const double rou = 180 / Math.PI * 3600;//206265
        protected double reg_alpha;//半参数估计的正则化参数

        protected Proc_t global_popt;//存储全局参数
        protected Weight WgtDef;
        protected double Eps_X, Eps_VCE;
        protected int DisWgt,IterMax_VCE;

        //创建矩阵
        public static mat Newmat(int m, int n)
        {
            return mat.Build.Dense(m, n, 0);
        }
        public static mat Eye(int m)
        {
            double[] diag = new double[m];
            for (int i = 0; i < m; i++) diag[i] = 1;
            return mat.Build.DiagonalOfDiagonalArray(diag);
        }

        //陀螺坐标方位角的权
        // ref [12],[13]
        mat GetPt(int nt, double sigma,Proc_t popt)
        {
            double[] diag = new double[nt];
            double adp_out,adp_in=1;
            int i = 0;
            foreach (GyroAzi t in Gyro)
            {
                if(popt.GyOpt.GyInAdp==1) adp_in = t.adpin;
                adp_out = popt.GyOpt.GyOuAdp;
                diag[i] = adp_in * adp_out * Math.Pow(sigma / sigt, 2);
                i++;
            }
            Pt = mat.Build.DiagonalOfDiagonalArray(diag);
            return Pt;
        }
        //计算定向角未知数
        protected void Orientation_Angle(out obs_dic DXJ)
        {
            DXJ = new obs_dic();
            //逐测站计算定向角未知数
            foreach (KeyValuePair<string, obs_dic> sta in obsL)
            {
                int i = 0, sign = 0, nl, ids;
                ids = FindStaName(sta.Key);
                nl = AllObs[ids].nl;
                double[] dir0 = new double[nl];
                sign = 0;//标志着本侧站的定向角是否一定程度接近0°（360°)
                foreach (KeyValuePair<string, double> obs in sta.Value)
                {
                    double dx = Result[obs.Key].X - Result[sta.Key].X;
                    double dy = Result[obs.Key].Y - Result[sta.Key].Y;
                    double azi = GetAzi(dx, dy);
                    dir0[i] = Keep_0To360(RD2D(azi) - DMS2D(obs.Value));//计算定向角(=照准点方位角-其观测角度）
#if true
                    //50M的测距，1M的Y点位偏差，最大产生1°的方位角偏差
                    if ((360 - dir0[i]) < 1) //如果定向角接近坐标北方向,避免方位角时而1，时而359；
                    { sign = 1; }
                    if (sign == 1)
                    {
                        if (dir0[i] > 359 && dir0[i] < 360)
                            dir0[i] = dir0[i] - 360;
                    }
#endif
                    i++;
                }
                double azi0 = Mean(dir0);
                DXJ.Add(sta.Key, azi0);
            }
        }
        //陀螺边误差方程 
        void GetTL(ref mat At, ref mat Lt)
        {
            int i = 0;
            foreach (GyroAzi gy in Gyro)
            {
                double dx = Result[gy.end].X - Result[gy.beg].X;
                double dy = Result[gy.end].Y - Result[gy.beg].Y;
                double azi = GetAzi(dx, dy);
                double S = Math.Sqrt(dx * dx + dy * dy);//距离的平方
                double a = rou * Math.Sin(azi) / S * 0.001;
                double b = rou * Math.Cos(azi) / S * 0.001;
                if (!knownPoint.ContainsKey(gy.beg) && !knownPoint.ContainsKey(gy.end))
                {
                    int j = ukpIndex[gy.beg]; //测站点在未知点中的编号
                    int k = ukpIndex[gy.end];//照准点在未知点中的编号
                    At[i, 2 * j] = a; //方向方程系数阵（"/mm)
                    At[i, 2 * j + 1] = -1.0 * b;
                    At[i, 2 * k] = -1.0 * a;
                    At[i, 2 * k + 1] = b;
                }
                else if (!knownPoint.ContainsKey(gy.beg) && knownPoint.ContainsKey(gy.end))
                { //观测点是已知点，测站是未知点
                    int j = ukpIndex[gy.beg]; //测站点在未知点中的编号
                    At[i, 2 * j] = a; //方向方程系数阵（"/mm)
                    A1[i, 2 * j + 1] = -1.0 * b;
                }
                else if (knownPoint.ContainsKey(gy.beg) && !knownPoint.ContainsKey(gy.end))
                {//观测点是未知点，测站是已知点
                    int k = ukpIndex[gy.end];//照准点在未知点中的编号
                    At[i, 2 * k] = -1.0 * a;
                    At[i, 2 * k + 1] = b;
                }
                else if (knownPoint.ContainsKey(gy.beg) && knownPoint.ContainsKey(gy.end))
                {  //两端都是已知点
                }
                Lt[i, 0] = (DMS2D(gy.gyazi) - RD2D(azi)) * 3600;
                i++;
            }
        }
        // 判断是否沿隧道侧壁
        protected int FindSide(string staName,string obsName)
        {
            int ids = FindStaName(staName);
            if (ids != -1)
            {
                int ido = FindObsName(ids, obsName);
                if (ido != -1)
                {
                    return AllObs[ids].data[ido].side;
                }
            }
            return 0;
        }
        // 区分沿隧道侧壁方向观测值 和 其它
        protected void GetFX2(obs_dic DXJ,out mat A1_,out mat L1_,out mat A2_,out mat L2_)
        {
            int i1 = 0; //all观测点计数
            int s = 0;//定向角计数
            int i2 = 0;
            int side;
            mat A1, L1, A2, L2;
            A1 = Newmat(nl, t);L1 = Newmat(nl, 1);
            A2 = Newmat(nl, t);L2 = Newmat(nl, 1);
            
            //参数的前2*ukp_num行表示未知点（x1,y1,x2,y2...），后CZ.Count行表示每个测站的定向角参数
            foreach (KeyValuePair<string, obs_dic> sta in obsL)//测站循环,计算误差方程矩阵
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)//照准点循环
                {
                    side = FindSide(sta.Key, obs.Key);
                    double dx = Result[obs.Key].X - Result[sta.Key].X;
                    double dy = Result[obs.Key].Y - Result[sta.Key].Y;
                    double azi = GetAzi(dx, dy);
                    double S = Math.Sqrt(dx * dx + dy * dy);//距离
                    double a = rou * Math.Sin(azi) / S * 0.001;
                    double b = rou * Math.Cos(azi) / S * 0.001;
                    if (!knownPoint.ContainsKey(sta.Key) && !knownPoint.ContainsKey(obs.Key))
                    {//两端都是未知点
                        int j = ukpIndex[sta.Key]; //测站点在未知点中的编号
                        int k = ukpIndex[obs.Key];//照准点在未知点中的编号
                        if (Math.Abs(side) == 2)
                        {
                            A1[i1, 2 * j] = a; //方向方程系数阵（"/mm)
                            A1[i1, 2 * j + 1] = -1.0 * b;
                            A1[i1, 2 * k] = -1.0 * a;
                            A1[i1, 2 * k + 1] = b;
                        }
                        else
                        {
                            A2[i2, 2 * j] = a; //方向方程系数阵（"/mm)
                            A2[i2, 2 * j + 1] = -1.0 * b;
                            A2[i2, 2 * k] = -1.0 * a;
                            A2[i2, 2 * k + 1] = b;
                        }
                    }
                    else if (!knownPoint.ContainsKey(sta.Key) && knownPoint.ContainsKey(obs.Key))
                    { //观测点是已知点，测站是未知点
                        int j = ukpIndex[sta.Key]; //测站点在未知点中的编号
                        if (Math.Abs(side) ==2)
                        {
                            A1[i1, 2 * j] = a; //方向方程系数阵（"/mm)
                            A1[i1, 2 * j + 1] = -1.0 * b;
                        }
                        else
                        {
                            A2[i2, 2 * j] = a; //方向方程系数阵（"/mm)
                            A2[i2, 2 * j + 1] = -1.0 * b;
                        }
                    }
                    else if (knownPoint.ContainsKey(sta.Key) && !knownPoint.ContainsKey(obs.Key))
                    {//观测点是未知点，测站是已知点
                        int k = ukpIndex[obs.Key];//照准点在未知点中的编号
                        if (Math.Abs(side) ==2)
                        {
                            A1[i1, 2 * k] = -1.0 * a;
                            A1[i1, 2 * k + 1] = b;
                        }
                        else
                        {
                            A2[i2, 2 * k] = -1.0 * a;
                            A2[i2, 2 * k + 1] = b;
                        }
                    }
                    else if (knownPoint.ContainsKey(sta.Key) && knownPoint.ContainsKey(obs.Key))
                    {  //两端都是已知点
                    }
                    if (Math.Abs(side)==2)
                        A1[i1, 2 * ukp_num + s] = -1.0;//定向角未知数对应的列系数，放在所有位置参数（XY）之后  
                    else
                        A2[i2, 2 * ukp_num + s] = -1.0;
                  
                    double L = Keep_0To360(DMS2D(obs.Value) + DXJ[sta.Key]);//方位角观测值(角度+定向角)
                    double L0 = azi * 180 / Math.PI;//反算的方位角
                    double dL = L - L0;
#if true
                    if (L > 359 && L0 < 1) { dL -= 360; }
                    else if (L < 1 && L0 > 359) { dL += 360; }
#endif
                    if (Math.Abs(side) ==2)
                        L1[i1++, 0] = dL * 3600;
                    else
                        L2[i2++, 0] = dL * 3600;
                }
                s++;//进入下一个测站
            }
            int r, c;
            A1_ = Newmat(i1, t); L1_ = Newmat(i1, 1);
            A2_ = Newmat(i2, t); L2_ = Newmat(i2, 1);
            for (r = 0; r < i1; r++)
            {
                for (c = 0; c < t; c++)
                {
                    A1_[r, c] = A1[r, c];
                }
                L1_[r, 0] = L1[r, 0];
            }
            for (r = 0; r < i2; r++)
            {
                for (c = 0; c < t; c++)
                {
                    A2_[r, c] = A2[r, c];
                }
                L2_[r, 0] = L2[r, 0];
            }
        }
        //方向方程矩阵(方位角参与平差）
        protected void GetFX1(obs_dic DXJ, ref mat A1, ref mat L1)
        {

            int i = 0; //all观测点计数
            int s = 0;//定向角计数
            //参数的前2*ukp_num行表示未知点（x1,y1,x2,y2...），后CZ.Count行表示每个测站的定向角参数
            foreach (KeyValuePair<string, obs_dic> sta in obsL)//测站循环,计算误差方程矩阵
            {
                /*
                string[] str = { "3", "6", "7", "53", "80" };
                if (Strstr(sta.Key, str) == 1)
                { }
                */

                foreach (KeyValuePair<string, double> obs in sta.Value)//照准点循环
                {
                    double dx = Result[obs.Key].X - Result[sta.Key].X;
                    double dy = Result[obs.Key].Y - Result[sta.Key].Y;
                    double azi = GetAzi(dx, dy);
                    double S = Math.Sqrt(dx * dx + dy * dy);//距离
                    double a = rou * Math.Sin(azi) / S * 0.001;
                    double b = rou * Math.Cos(azi) / S * 0.001;
                    if (!knownPoint.ContainsKey(sta.Key) && !knownPoint.ContainsKey(obs.Key))
                    {//两端都是未知点
                        int j = ukpIndex[sta.Key]; //测站点在未知点中的编号
                        int k = ukpIndex[obs.Key];//照准点在未知点中的编号
                        A1[i, 2 * j] = a; //方向方程系数阵（"/mm)
                        A1[i, 2 * j + 1] = -1.0 * b;
                        A1[i, 2 * k] = -1.0 * a;
                        A1[i, 2 * k + 1] = b;
                    }
                    else if (!knownPoint.ContainsKey(sta.Key) && knownPoint.ContainsKey(obs.Key))
                    { //观测点是已知点，测站是未知点
                        int j = ukpIndex[sta.Key]; //测站点在未知点中的编号
                        A1[i, 2 * j] = a; //方向方程系数阵（"/mm)
                        A1[i, 2 * j + 1] = -1.0 * b;
                    }
                    else if (knownPoint.ContainsKey(sta.Key) && !knownPoint.ContainsKey(obs.Key))
                    {//观测点是未知点，测站是已知点
                        int k = ukpIndex[obs.Key];//照准点在未知点中的编号
                        A1[i, 2 * k] = -1.0 * a;
                        A1[i, 2 * k + 1] = b;
                    }
                    else if (knownPoint.ContainsKey(sta.Key) && knownPoint.ContainsKey(obs.Key))
                    {  //两端都是已知点
                        //continue;  //怎么敢continue的？!
                    }
                    A1[i, 2 * ukp_num + s] = -1.0;//定向角未知数对应的列系数，放在所有位置参数（XY）之后  
                    double L = Keep_0To360(DMS2D(obs.Value) + DXJ[sta.Key]);//方位角观测值(角度+定向角)
                    double L0 = azi * 180 / Math.PI;//反算的方位角
                    double dL = L-L0;
#if true
                    if (L > 359 && L0 < 1) { dL -= 360 ; }
                    else if (L < 1 && L0 > 359) { dL += 360 ; }
#endif
                    L1[i, 0] = dL * 3600;
                    i++;//进入下一个照准点（不是观测值）
                }
                s++;//进入下一个测站
            }
        }
        //距离方程矩阵，估计定向角未知数
        protected void GetJL1(ref mat A2, ref mat L2)
        {
            int i = 0; //距离观测值计数
            int s = 0;//测站计数 
            foreach (KeyValuePair<string, obs_dic> sta in obsS)//测站循环,计算误差方程矩阵
            {
                //string[] str = { "3","6","7","53", "80" };
                //if (Strstr(sta.Key,str)==1)
                //{ }
                //if(i==26)
                //{ }
                foreach (KeyValuePair<string, double> obs in sta.Value)//照准点循环
                {
                    double dx = Result[obs.Key].X - Result[sta.Key].X;
                    double dy = Result[obs.Key].Y - Result[sta.Key].Y;
                    double S = Math.Sqrt(dx * dx + dy * dy);
                    double c = dx / S;
                    double d = dy / S;
                    if (!knownPoint.ContainsKey(sta.Key) && !knownPoint.ContainsKey(obs.Key))
                    {//两端都是未知点
                        int j = ukpIndex[sta.Key]; //测站点在未知点中的编号
                        int k = ukpIndex[obs.Key];//照准点在未知点中的编号
                        A2[i, 2 * j] = -1.0 * c;//距离方程系数阵(1)
                        A2[i, 2 * j + 1] = -1.0 * d;
                        A2[i, 2 * k] = c;
                        A2[i, 2 * k + 1] = d;
                    }
                    else if (!knownPoint.ContainsKey(sta.Key) && knownPoint.ContainsKey(obs.Key))
                    { //观测点是已知点，测站是未知点
                        int j = ukpIndex[sta.Key]; //测站点在未知点中的编号
                        A2[i, 2 * j] = -1.0 * c;//距离方程系数阵(1)
                        A2[i, 2 * j + 1] = -1.0 * d;
                    }
                    else if (knownPoint.ContainsKey(sta.Key) && !knownPoint.ContainsKey(obs.Key))
                    {//观测点是未知点，测站是已知点
                        int k = ukpIndex[obs.Key];//照准点在未知点中的编号
                        A2[i, 2 * k] = c;
                        A2[i, 2 * k + 1] = d;
                    }
                    else if (knownPoint.ContainsKey(sta.Key) && knownPoint.ContainsKey(obs.Key))
                    {  //两端都是已知点
                       //continue;
                    }
                    double dL = obs.Value - S;
                    L2[i, 0] = 1000 * dL;//距离方程常数项(mm)（=观测值-计算值）
                    i++;//进入下一个观测点（不是观测值）
                }
                s++;//进入下一个测站
            }
        }
        //方向权阵
        protected mat GetP1(int nl, double sigma0)
        {
            double[] diag = new double[nl];
            for (int p = 0; p < nl; p++)
                diag[p] = Math.Pow(sigma0 / alpha, 2);//方向权阵
            P1 = mat.Build.DiagonalOfDiagonalArray(diag);
            return P1;
        }
        //距离权阵
        protected mat GetP2(int ns, double sigma0, int DisWgt)
        {
            int i = 0;//观测排序
            double[] diag = new double[ns];
            foreach (KeyValuePair<string, obs_dic> sta in obsS)
            {
                string staName = sta.Key;
                int ids = FindStaName(sta.Key);
                foreach (KeyValuePair<string, double> obs in sta.Value)//照准点循环
                {
                    int ido = FindObsName(ids, obs.Key);
                    double cg = 0;
                    if (AllObs[ids].data[ido].comego == 1 && global_popt.AveDis==1 )
                        cg = 1;
                    string obsName = obs.Key;
                    double ms2 = 0;//
                    if (cg==1)
                    {
                        double S1 = Dis(staName, obsName)*1e-3;
                        double S2 = Dis(obsName, staName)*1e-3;
                        if (DisWgt == 1)
                            ms2 = 0.25 * (Math.Pow(msa + msb * S1, 2) + Math.Pow(msa + msb * S2, 2));
                        else
                            ms2 = 0.25 * (msa * msa + msb * msb * S1 * S1 + msa * msa + msb * msb * S2 * S2);
                    }
                    else
                    {
                        double S = obs.Value * 1e-3; //距离，单位是千米
                        if (DisWgt == 1) { ms2 = Math.Pow(msa + msb * S, 2); }
                        else { ms2 = msa * msa + msb * msb * S * S; }
                    }
                    double adp = 1;
#if false
                    if (global_param.DisAdp != 0)
                        adp = global_param.DisAdp;
#endif
                    diag[i] =  adp * Math.Pow(sigma0,2) / ms2;//距离权阵
                    i++;
                }
            }

            //P2.SetDiagonal(diag);
            P2 = mat.Build.DiagonalOfDiagonalArray(diag);
            return P2;
        }

        // 引入定向角，建立误差方程矩阵,验前验后估计
        protected void Adjust(Proc_t popt)
        {
            int i,iter = 0; //iter坐标迭代次数;
            int converge = 0;//迭代是否收敛
            mat Xprev = Newmat(t, 1); //存储上一次参数解
            mat X1 ; //只迭代一次的参数解，不收敛时使用

            cov_res = new List<VCE_t>();//存储方差分量比值
            iter_VCE = 1;//方差分量估计计数

            if (Approx.Count < PointName.Count) return;
            Result = new poi_dic(Approx);//近似坐标作为迭代的初值

            P = Newmat(n, n); QX = Newmat(t, t); mat ROC = Newmat(n, n);
            N1 = Newmat(t, t); W1 = Newmat(t, 1);
            N2 = Newmat(t, t); W2 = Newmat(t, 1);
            N = Newmat(t, t); W = Newmat(t, 1);
            X = Newmat(t, 1); V1 = Newmat(nl, 1); V2 = Newmat(ns, 1);
            //tuoluo
            Nt = Newmat(t, t); Wt = Newmat(t, 1);

            //传参
            WgtDef = popt.WgtDef;//定权方法
            DisWgt = popt.DisWgt;//距离定权公式
            Eps_X = popt.Eps_X;//坐标迭代限差
            Eps_VCE = popt.Eps_VCE;//方差分量估计迭代限差
            IterMax_VCE = popt.IterMax_VCE;  //定权迭代最大次数
            var GyOpt = popt.GyOpt;

            M0 = alpha;
            //方差因子初值
            qv = M0 * M0;
            qvl = qv;
            qvs = qv;

            //int nt = TL.Count;//陀螺边观测数
            P1 = Newmat(nl, nl); P2 = Newmat(ns, ns); Pt = Newmat(nt, nt);

            //定初权
            P1 = GetP1(nl, M0);
            P2 = GetP2(ns, M0, DisWgt);
            P = P1.DiagonalStack(P2);
            if (GyOpt.UseGyro == 1)
            {
                Pt = GetPt(nt, M0,popt);
                P = P1.DiagonalStack(P2).DiagonalStack(Pt);
            }

            A1 = Newmat(nl, t); L1 = Newmat(nl, 1);
            A2 = Newmat(ns, t); L2 = Newmat(ns, 1);

            //建立误差方程矩阵
            //定向角：测站第一个照准方向的方位角
            Orientation_Angle(out obs_dic DXJ);//计算定向角
            GetFX1(DXJ, ref A1, ref L1);
            GetJL1(ref A2, ref L2);

            var At = Newmat(nt, t);  var Lt = Newmat(nt, 1);
            if (GyOpt.UseGyro == 1) 
                GetTL(ref At, ref Lt);

            //法方程矩阵
            N1 = A1.Transpose() * P1 * A1;
            W1 = A1.Transpose() * P1 * L1;
            N2 = A2.Transpose() * P2 * A2;
            W2 = A2.Transpose() * P2 * L2;
            N = N1 + N2;
            W = W1 + W2;
            if (GyOpt.UseGyro == 1)
            {
                Nt = At.Transpose() * Pt * At;
                Wt = At.Transpose() * Pt * Lt;
                N = N1 + N2 + Nt;
                W = W1 + W2 + Wt;
            }

            X = N.Inverse() * W;//未知参数改正数
            X1 = X;

            if (popt.WgtDef != Weight.Tradition)
            {
                V1 = A1 * X - L1;
                V2 = A2 * X - L2;
                if (GyOpt.UseGyro == 1) Vt = At * X - Lt;
                PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
                PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
            }

            //外层迭代坐标，内层迭代单位权方差
            while (true)  //迭代近似坐标，误差方程变化
            {
                //内循环迭代方差因子
                switch (popt.WgtDef)
                {
                    case Weight.Helmert: Helmert();break;//赫尔默特方差分量估计定权
                    case Weight.hmt1: SimplfyHe();break;
                    case Weight.hmt2:SimplfyHe2();break;
                    case Weight.hmt3:SimplfyHe3();break;
                    case Weight.MINQUE:MINQUE();break;
                    case Weight.LSVCE:LSVCE();break;
                    case Weight.MLE:MLE();break;
                    default:break;
                }
                //重新平差
                N1 = A1.Transpose() * P1 * A1;
                W1 = A1.Transpose() * P1 * L1;
                N2 = A2.Transpose() * P2 * A2;
                W2 = A2.Transpose() * P2 * L2;
                N = N1 + N2;
                W = W1 + W2;
                if (GyOpt.UseGyro == 1)
                {
                    Nt = At.Transpose() * Pt * At;
                    Wt = At.Transpose() * Pt * Lt;
                    N = N1 + N2 + Nt;
                    W = W1 + W2 + Wt;
                }

                X = N.Inverse() * W;//未知参数改正数
                foreach (KeyValuePair<string, int> dic in ukpIndex)
                {
                    Result[dic.Key].X += X[2 * dic.Value, 0] / 1000;
                    Result[dic.Key].Y += X[2 * dic.Value + 1, 0] / 1000;
                }

                if (popt.WgtDef != Weight.Tradition)
                {
                    V1 = A1 * X - L1;
                    V2 = A2 * X - L2;
                    if (GyOpt.UseGyro == 1) Vt = At * X - Lt;
                    PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
                    PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
                }

                double eps,epsMax=0;
                for (i = 0; i < t; i++)
                {
                    eps = Math.Abs(X[i, 0] - Xprev[i, 0]);
                    if (eps > epsMax) epsMax = eps;
                }
                //跳出外循环
                if (epsMax < popt.Eps_X)  //判断是否结束近似坐标迭代
                {
                    //存储方差分量估计的定权结果(最终的）
                    VCE_t w = new VCE_t
                    {
                        count = iter_VCE,
                        ql = qvl,
                        qs = qvs
                    };
                    cov_res.Add(w);
                    break;
                }

                //如果不收敛，避免死锁
                if (iter == popt.IterMax_X)
                {
                    //如果不收敛，选择迭代一次的解
                    //Result = Approx;//弃用，Approx可能已经改变，因为浅拷贝
                    for (i = 0; i < Approx.Count; i++)
                    {
                        string key = CopyApprox[i].name;
                        double x = CopyApprox[i].x;
                        double y = CopyApprox[i].y;
                        Result[key].X = x;
                        Result[key].Y = y;
                    }
                    foreach (KeyValuePair<string, int> dic in ukpIndex)
                    {
                        Result[dic.Key].X += X1[2 * dic.Value, 0] / 1000;
                        Result[dic.Key].Y += X1[2 * dic.Value + 1, 0] / 1000;
                    }
                    converge = -1;
                    break;
                }
                iter++;
                Xprev = X; 
                //如果没达到迭代阈值，重新建立误差方程；坐标迭代后，误差方程矩阵也跟着改变
                Orientation_Angle(out DXJ);//计算定向角
                GetFX1(DXJ, ref A1, ref L1);
                GetJL1(ref A2, ref L2);
                if (GyOpt.UseGyro == 1)
                    GetTL(ref At, ref Lt);
            }
            /*如果收敛，最小的改正数就在最后一次迭代
             *如果不收敛，最小改正数就是极小值点*/
            if (converge == -1) 
                X = X1;

            //计算多余观测分量矩阵
            if (GyOpt.UseGyro == 1)
            {
                A = A1.Stack(A2).Stack(At);
                L = L1.Stack(L2).Stack(Lt);
                P = P1.DiagonalStack(P2).DiagonalStack(Pt);
            }
            else
            {
                A = A1.Stack(A2);
                L = L1.Stack(L2);
                P = P1.DiagonalStack(P2);
            }
            QX = N.Inverse();//参数协因数
            //ROC = Eye(n) - A * QX * A.Transpose() * P;//多余观测分量Qvv*P
            var Qvv= P.Inverse() - A * QX * A.Transpose();
            ROC = Qvv * P;

            //  存储信息
            i = 0; Q1 = P1.Inverse();
            foreach (KeyValuePair<string, obs_dic> sta in obsL)
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)
                {
                    int ids = FindStaName(sta.Key);
                    int ido = FindObsName(ids, obs.Key);
                    double v = (A1 * X)[i, 0] - L1[i, 0];
                    V1[i, 0] = v;
                    AllObs[ids].data[ido].vl = v;
                    AllObs[ids].data[ido].qL = Q1[i, i];
                    AllObs[ids].data[ido].rocL = ROC[i, i];
                    i++;
                }
            }
            i = 0; Q2 = P2.Inverse();
            foreach (KeyValuePair<string, obs_dic> sta in obsS)
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)
                {
                    int ids = FindStaName(sta.Key);
                    int ido = FindObsName(ids, obs.Key);
                    double v = (A2 * X)[i, 0] - L2[i, 0];
                    V2[i, 0] = v;
                    AllObs[ids].data[ido].vs = v;
                    AllObs[ids].data[ido].qS = Q2[i, i];
                    AllObs[ids].data[ido].rocS = ROC[nl+i, nl+i];
                    i++;
                }
            }
            if (GyOpt.UseGyro == 1)
            {
                Qt = Pt.Inverse();
                for (i = 0; i < nt; i++)
                {
                    int ids = FindStaName(Gyro[i].beg);
                    int ido = FindObsName(ids, Gyro[i].end);
                    double v = (At * X)[i, 0] - Lt[i, 0];
                    AllObs[ids].data[ido].vt = v;
                    AllObs[ids].data[ido].qT = Qt[i, i];
                    AllObs[ids].data[ido].rocT = ROC[nl + ns + i, nl + ns + i];
                }
            }

            //计算验后单位权中误差
            V1 = A1 * X - L1;
            V2 = A2 * X - L2;
            PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
            PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
            PVV = PVV1 + PVV2;
            if (GyOpt.UseGyro == 1)
            {
                Vt = At * X - Lt;
                PVVt = (Vt.Transpose() * Pt * Vt)[0, 0];
                PVV = PVV1 + PVV2 + PVVt;
            }
            r = n - t;
            sigma = Math.Sqrt(PVV / r);
            if (popt.WgtDef == Weight.Tradition) fai = M0;
            else fai = sigma; //验后单位权中误差

            Print("tr(ROC)=" + ROC.Trace());
            Print("iter counts : " + iter);
        }
    }
}
